.equ    Mode_USR,       0x10
.equ    Mode_FIQ,       0x11
.equ    Mode_IRQ,       0x12
.equ    Mode_SVC,       0x13
.equ    Mode_ABT,       0x17
.equ    Mode_UND,       0x1B
.equ    Mode_SYS,       0x1F

.equ    I_Bit,          0x80
.equ    F_Bit,          0x40

.equ    UND_Stack_Size, 0x00000000
.equ    SVC_Stack_Size, 0x00000008
.equ    ABT_Stack_Size, 0x00000000
.equ    FIQ_Stack_Size, 0x00000000
.equ    IRQ_Stack_Size, 0x00000100
.equ    USR_Stack_Size, 0x00000400

.equ    ISR_Stack_Size, (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + FIQ_Stack_Size + IRQ_Stack_Size)

.equ    Heap_Size, 0x00000000

.equ    SCB_BASE, 0xE01FC000
.equ    PLLCON_OFS, 0x80
.equ    PLLCFG_OFS, 0x84
.equ    PLLSTAT_OFS, 0x88
.equ    PLLFEED_OFS, 0x8C
.equ    CCLKCFG_OFS, 0x104
.equ    USBCLKCFG_OFS, 0x108
.equ    CLKSRCSEL_OFS, 0x10C
.equ    SCS_OFS, 0x1A0
.equ    PCLKSEL0_OFS, 0x1A8
.equ    PCLKSEL1_OFS, 0x1AC


.equ    OSCRANGE, (1<<4)
.equ    OSCEN, (1<<5)
.equ    OSCSTAT, (1<<6)
.equ    PLLCON_PLLE, (1<<0)
.equ    PLLCON_PLLC, (1<<1)
.equ    PLLSTAT_M, (0x7FFF<<0)
.equ    PLLSTAT_N, (0xFF<<16)
.equ    PLLSTAT_PLOCK, (1<<26)

.equ    CLOCK_SETUP, 1
.equ    SCS_Val, 0x00000020
.equ    CLKSRCSEL_Val, 0x00000001
.equ    PLLCFG_Val, 0x0000000B
.equ    CCLKCFG_Val, 0x00000005
.equ    USBCLKCFG_Val, 0x00000005
.equ    PCLKSEL0_Val, 0x00000000
.equ    PCLKSEL1_Val, 0x00000000


.equ    MAM_BASE, 0xE01FC000
.equ    MAMCR_OFS, 0x00
.equ    MAMTIM_OFS, 0x04

.equ    MAM_SETUP, 1
.equ    MAMCR_Val, 0x00000002
.equ    MAMTIM_Val, 0x00000004

                .section STACK, "w"
Stack_Mem:		.space USR_Stack_Size
__initial_sp:	.space ISR_Stack_Size
Stack_Top:
				.section HEAP,"w"	
__heap_base:
Heap_Mem:		.space Heap_Size
__heap_limit:
              
                .section RESET ,"ax"
                .arm

Vectors:        LDR     PC, Reset_Addr         
                LDR     PC, Undef_Addr
                LDR     PC, SWI_Addr
                LDR     PC, PAbt_Addr
                LDR     PC, DAbt_Addr
                NOP                             
                LDR     PC, [PC, #-0x0120]     
                LDR     PC, FIQ_Addr

Reset_Addr:      .word     Reset_Handler
Undef_Addr:      .word     Undef_Handler
SWI_Addr:        .word     SWI_Handler
PAbt_Addr:       .word     PAbt_Handler
DAbt_Addr:       .word     DAbt_Handler
                 .word     0                       
IRQ_Addr:        .word     IRQ_Handler
FIQ_Addr:        .word     FIQ_Handler

Undef_Handler:   B       Undef_Handler
SWI_Handler:     B       SWI_Handler
PAbt_Handler:    B       PAbt_Handler
DAbt_Handler:    B       DAbt_Handler
IRQ_Handler:     B       IRQ_Handler
FIQ_Handler:     B       FIQ_Handler

                .global  Reset_Handler
Reset_Handler:   

                .if      CLOCK_SETUP != 0
                LDR     R0, =SCB_BASE
                MOV     R1, #0xAA
                MOV     R2, #0x55

                LDR     R3, =SCS_Val          
                STR     R3, [R0, #SCS_OFS] 

                .if      (SCS_Val && OSCEN) != 0  
OSC_Loop:       LDR     R3, [R0, #SCS_OFS]    
                ANDS    R3, R3, #OSCSTAT
                BEQ     OSC_Loop
                .endif

                LDR     R3, =CLKSRCSEL_Val    
                STR     R3, [R0, #CLKSRCSEL_OFS] 
                LDR     R3, =PLLCFG_Val
                STR     R3, [R0, #PLLCFG_OFS] 
                STR     R1, [R0, #PLLFEED_OFS]
                STR     R2, [R0, #PLLFEED_OFS]
                MOV     R3, #PLLCON_PLLE
                STR     R3, [R0, #PLLCON_OFS]
                STR     R1, [R0, #PLLFEED_OFS]
                STR     R2, [R0, #PLLFEED_OFS]

PLL_Loop:       LDR     R3, [R0, #PLLSTAT_OFS]
                ANDS    R3, R3, #PLLSTAT_PLOCK
                BEQ     PLL_Loop

M_N_Lock:       LDR     R3, [R0, #PLLSTAT_OFS]
                LDR     R4, =(PLLSTAT_M||PLLSTAT_N)
                AND     R3, R3, R4
                LDR     R4, =PLLCFG_Val
                EORS    R3, R3, R4
                BNE     M_N_Lock

                MOV     R3, #CCLKCFG_Val
                STR     R3, [R0, #CCLKCFG_OFS]

                LDR     R3, =USBCLKCFG_Val
                STR     R3, [R0, #USBCLKCFG_OFS]

                LDR     R3, =PCLKSEL0_Val
                STR     R3, [R0, #PCLKSEL0_OFS]
                LDR     R3, =PCLKSEL1_Val
                STR     R3, [R0, #PCLKSEL1_OFS]

                MOV     R3, #(PLLCON_PLLE||PLLCON_PLLC)
                STR     R3, [R0, #PLLCON_OFS]
                STR     R1, [R0, #PLLFEED_OFS]
                STR     R2, [R0, #PLLFEED_OFS]
                .endif  

                .if     MAM_SETUP != 0
                LDR     R0, =MAM_BASE
                MOV     R1, #MAMTIM_Val
                STR     R1, [R0, #MAMTIM_OFS] 
                MOV     R1, #MAMCR_Val
                STR     R1, [R0, #MAMCR_OFS] 
                .endif   

.equ    MEMMAP,0xE01FC040    
                .ifdef     REMAP
                LDR     R0, =MEMMAP
                .ifdef  RAM_MODE
                MOV     R1, #2
                .else
                MOV     R1, #1
                .endif
                STR     R1, [R0]
                .endif

                LDR     R0, =Stack_Top

                MSR     CPSR_c, #Mode_UND||I_Bit||F_Bit
                MOV     SP, R0
                SUB     R0, R0, #UND_Stack_Size

                MSR     CPSR_c, #Mode_ABT||I_Bit||F_Bit
                MOV     SP, R0
                SUB     R0, R0, #ABT_Stack_Size

                MSR     CPSR_c, #Mode_FIQ||I_Bit||F_Bit
                MOV     SP, R0
                SUB     R0, R0, #FIQ_Stack_Size

                MSR     CPSR_c, #Mode_IRQ||I_Bit||F_Bit
                MOV     SP, R0
                SUB     R0, R0, #IRQ_Stack_Size

                MSR     CPSR_c, #Mode_SVC||I_Bit||F_Bit
                MOV     SP, R0
                SUB     R0, R0, #SVC_Stack_Size

                MSR     CPSR_c, #Mode_USR
                .ifdef  __MICROLIB

                .global __initial_sp

                .else

                MOV     SP, R0
                SUB     SL, SP, #USR_Stack_Size

                .endif


                .extern  main
                LDR     R0, =main
                BX      R0


                .ifdef  __MICROLIB

                EXPORT  __heap_base
                EXPORT  __heap_limit

                .else
				.section .text,"ax"
                .extern  __use_two_region_memory
                .global  __user_initial_stackheap
__user_initial_stackheap:

                LDR     R0, =  Heap_Mem
                LDR     R1, =(Stack_Mem + USR_Stack_Size)
                LDR     R2, = (Heap_Mem +      Heap_Size)
                LDR     R3, = Stack_Mem
                BX      LR
                .endif
                
                .end
 